/*
 * servo.c
 *
 *  Created on: 2017年3月5日
 *      Author: chen
 */

/* Includes ------------------------------------------------------------------*/
#include "servo.h"

#include "fsl_ftm.h"
#include "peripherals.h"

void FTM_UpdatePwmDuty(FTM_Type *base,
                            ftm_chnl_t chnlNumber,
                            ftm_pwm_mode_t currentPwmMode,
                            uint16_t dutyCycle);

uint16_t SD5_Conf[3] = {210, 3910, 6090};   //{舵机中值偏差, 左打角限制, 右打角限制}

/* User Codes ----------------------------------------------------------------*/
void SD5_Config(void)
{
    FTM_UpdatePwmDuty(SERVO_PERIPHERAL, kFTM_Chnl_1, 
                      kFTM_EdgeAlignedPwm, 5000 + SD5_Conf[0]);
    FTM_SetSoftwareTrigger(SERVO_PERIPHERAL, true);
}

void SD5_ChangeDuty(uint16_t val)
{
	if (val<SD5_Conf[1])
        val = SD5_Conf[1];

	if (val>SD5_Conf[2])
        val = SD5_Conf[2];

    FTM_UpdatePwmDuty(SERVO_PERIPHERAL, kFTM_Chnl_1, 
                      kFTM_EdgeAlignedPwm, val + SD5_Conf[0]);
    FTM_SetSoftwareTrigger(SERVO_PERIPHERAL, true);
}


/* 
 * 10000分度占空比调节 
 * 重写占空比函数
 */
void FTM_UpdatePwmDuty(FTM_Type *base,
                            ftm_chnl_t chnlNumber,
                            ftm_pwm_mode_t currentPwmMode,
                            uint16_t dutyCycle)
{
    uint16_t cnv, cnvFirstEdge = 0, mod;

    mod = base->MOD;
    if ((currentPwmMode == kFTM_EdgeAlignedPwm) || (currentPwmMode == kFTM_CenterAlignedPwm))
    {
        cnv = (mod * dutyCycle) / 10000;
        /* For 100% duty cycle */
        if (cnv >= mod)
        {
            cnv = mod + 1;
        }
        base->CONTROLS[chnlNumber].CnV = cnv;
    }
    else
    {
        /* This check is added for combined mode as the channel number should be the pair number */
        if (chnlNumber >= (FSL_FEATURE_FTM_CHANNEL_COUNTn(base) / 2))
        {
            return;
        }

        cnv = (mod * dutyCycle) / 10000;
        cnvFirstEdge = base->CONTROLS[chnlNumber * 2].CnV;
        /* For 100% duty cycle */
        if (cnv >= mod)
        {
            cnv = mod + 1;
        }
        base->CONTROLS[(chnlNumber * 2) + 1].CnV = cnvFirstEdge + cnv;
    }
}
